#include "PowerManager.h"

void PowerManager::update(float batteryPct, unsigned long nowMs) {
  (void)nowMs;
  PowerMode previous = _mode;
  PowerMode next = _mode;

  switch (_mode) {
    case PowerMode::NORMAL:
      if (batteryPct <= Power::CRITICAL_ENTER_PCT)      next = PowerMode::CRITICAL;
      else if (batteryPct <= Power::CONSERVE_ENTER_PCT) next = PowerMode::CONSERVE;
      break;
    case PowerMode::CONSERVE:
      if (batteryPct <= Power::CRITICAL_ENTER_PCT)      next = PowerMode::CRITICAL;
      else if (batteryPct >= Power::CONSERVE_EXIT_PCT)  next = PowerMode::NORMAL;
      break;
    case PowerMode::CRITICAL:
      if (batteryPct >= Power::CRITICAL_EXIT_PCT)       next = PowerMode::CONSERVE;
      break;
  }

  _mode = next;
  _modeChangedThisUpdate = (next != previous);
}

float PowerManager::motorSpeedScale() const {
  switch (_mode) {
    case PowerMode::NORMAL:   return Power::SPEED_SCALE_NORMAL;
    case PowerMode::CONSERVE: return Power::SPEED_SCALE_CONSERVE;
    case PowerMode::CRITICAL: return Power::SPEED_SCALE_CRITICAL;
  }
  return Power::SPEED_SCALE_NORMAL;
}

unsigned long PowerManager::pollIntervalMs() const {
  switch (_mode) {
    case PowerMode::NORMAL:   return Power::POLL_INTERVAL_NORMAL_MS;
    case PowerMode::CONSERVE: return Power::POLL_INTERVAL_CONSERVE_MS;
    case PowerMode::CRITICAL: return Power::POLL_INTERVAL_CRITICAL_MS;
  }
  return Power::POLL_INTERVAL_NORMAL_MS;
}

unsigned long PowerManager::hopDurationMs() const {
  // Shorter hops in lower power modes: less distance covered per drive
  // pulse, but also less current drawn per pulse.
  switch (_mode) {
    case PowerMode::NORMAL:   return Motion::HOP_DURATION_MS;
    case PowerMode::CONSERVE: return (unsigned long)(Motion::HOP_DURATION_MS * 0.75f);
    case PowerMode::CRITICAL: return (unsigned long)(Motion::HOP_DURATION_MS * 0.55f);
  }
  return Motion::HOP_DURATION_MS;
}

bool PowerManager::shouldSkipThisPlantingOpportunity() {
  if (_mode != PowerMode::CRITICAL) return false;
  _skipToggle = !_skipToggle;
  return _skipToggle;
}

bool PowerManager::backlightShouldBeOn(unsigned long nowMs) const {
  if (_mode == PowerMode::NORMAL) return true;
  unsigned long half = Power::BACKLIGHT_DUTY_PERIOD_MS / 2;
  return ((nowMs / half) % 2) == 0;
}

const __FlashStringHelper* PowerManager::modeName() const {
  switch (_mode) {
    case PowerMode::NORMAL:   return F("NORMAL");
    case PowerMode::CONSERVE: return F("CONSERVE");
    case PowerMode::CRITICAL: return F("CRITICAL");
  }
  return F("UNKNOWN");
}
